﻿#include "dmkemotor.h"

DmkeMotor::DmkeMotor(int pulse_per_round, double length_per_pulse, 
    int left_direction, int right_direction, long acce, long dece)
{
    this->pulse_per_round    = pulse_per_round;
    this->length_per_pulse   = length_per_pulse;
    wheel_direction[NODE_INDEX::LEFT_NODE_ID]   = static_cast<char>(left_direction);
    wheel_direction[NODE_INDEX::RIGHT_NODE_ID]  = static_cast<char>(right_direction);
    this->acce = acce;
    this->dece = dece;
}
//DmkeMotor::~DmkeMotor(){}

//public
//frames数组大小定义要大于等于50
void DmkeMotor::DataFramesMotorInit(CanDataFrame *frames, uint8 &i)
{
    CanDataFrameOperationalMode( frames[ i++ ], NMT_MODE::NMT_OPERATION_MODE );
    CanDataFrameSpeedMode( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameSpeedMode( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    CanDataFrameAcce( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, acce );
    CanDataFrameAcce( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, acce );
    CanDataFrameDece( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, dece );
    CanDataFrameDece( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, dece );
    CanDataFrameControlByte0F( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameControlByte0F( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    //节点保护时间(单位:ms),这里设置 250，节点保护因子.这里设置 4
    //节点超时时间是 250*4=1000ms,如果在 1s 时间内没要收到远程帧,节点停止.
    CanDataFrameNodeProtectTimer( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, 250 );
    CanDataFrameNodeProtectFactor( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, 4 );
    CanDataFrameNodeProtectTimer( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, 250 );
    CanDataFrameNodeProtectFactor( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, 4 );
}

void DmkeMotor::DataFramesMotorSpeed(CanDataFrame *frames, uint8 &i, double speedl, double speedr)
{
    /*设置速度 公式说明：
    * “right_direction_” 因为电机安装位置反转，用来修正设定方向为同符号
    * “10 * rightSpeed” 设定左轮速度在最高速度，单位 mm/s
    * length_per_pulse 每个脉冲的距离 单位：mm/pulse
    * 乘10 ：驱动器的解析度为0.1 pulse ，驱动器解析值为整数。
    */
    long ls = static_cast< long >( wheel_direction[NODE_INDEX::LEFT_NODE_ID] * 10 * speedl / length_per_pulse ) * 10;
    long rs = static_cast< long >( wheel_direction[NODE_INDEX::RIGHT_NODE_ID] * 10 * speedr / length_per_pulse ) * 10;

    CanDataFrameSetSpeedValue( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, ls );
    CanDataFrameSetSpeedValue( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, rs );        
}

void DmkeMotor::DataFramesMotorRequest(CanDataFrame *frames, uint8 &i)
{
    CanDataFrameGetCurSpeedValue( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameGetCurSpeedValue( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    CanDataFrameGetCurPoseValue( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameGetCurPoseValue( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    CanDataFrameRemote( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameRemote( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );        
}

QString DmkeMotor::RecDataParse(CanDataFrame* frames, uint8 cnt) {
    QString msg;

    if(nullptr == frames || 0 >= cnt)
        return msg;

    uint32 uID;
    CanDataFrame* frame;
    for(int i=0; i<cnt; i++)
    {
        frame = &(frames[i]);
        uID = frame->uID;
        if(uID == (NODE_INDEX::LEFT_NODE_ID+CAN_ID::CAN_ID_REC))
            msg = DataParse(NODE_INDEX::LEFT_NODE_ID, frame->arryData, mfb_para.lmfp);
        else if(uID == (NODE_INDEX::RIGHT_NODE_ID+CAN_ID::CAN_ID_REC))
            msg = DataParse(NODE_INDEX::RIGHT_NODE_ID, frame->arryData, mfb_para.rmfp);
    }
    return msg;
}

DMotorFeedBackPara DmkeMotor::RetMotorFeedbackParam() {
    return mfb_para;
}

QString DmkeMotor::DataParse(NODE_INDEX index, uint8 (&array)[8], SMotorFeedBackPara &para)
{
    DATA_CONV dc;    
    QString msg;

    std::copy(std::begin(array),std::end(array),std::begin(dc.array));
    if(CS::CS_RESPOND_ERR == dc.val.cs) 
        return msg;
        
    //根据协议解析
    switch (dc.val.od_index)
    {
    case DO_INDEX::DO_READ_SPEED:
        para.speed = static_cast<int>(dc.val.val) * length_per_pulse * 0.1 * 0.001 * wheel_direction[index];
        break;
    case DO_INDEX::DO_READ_POS:
        para.pos = static_cast<int>(dc.val.val) * wheel_direction[index];
        break;
    case DO_INDEX::DO_READ_ERR:
        para.err = static_cast<int>(dc.val.val);
        msg = MotorErrInfo(index, para.err);
        break;
    default:
        break;
    }
    return msg;
}

//public
void DmkeMotor::DataFramesMotorEnable(CanDataFrame* frames, uint8 &i) {
    CanDataFrameControlByte0F( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameControlByte0F( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
}

void DmkeMotor::DataFramesMotorDisable(CanDataFrame* frames, uint8 &i) {
    CanDataFrameControlByte00( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameControlByte00( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
}

//全部伺服电机设置停止模式
void DmkeMotor::DataFramesStopMotor(CanDataFrame *frames, uint8 &i)
{
    CanDataFrameSetSpeedValue( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, 0 );
    CanDataFrameSetSpeedValue( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, 0 );
    CanDataFrameControlByte00( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameControlByte00( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
    CanDataFrameOperationalMode( frames[ i++ ], NMT_MODE::NMT_STOP_MODE );
}

void DmkeMotor::DataFramesAccDecUpdate(CanDataFrame *frames, uint8 &i, double& acc, double& dec ) {
    long m_acce = static_cast< long >( acc * pulse_per_round / 10 );
    long m_dece = static_cast< long >( dec * pulse_per_round / 10 );
    if ( m_acce == acce && m_dece == dece ) { return; }  //同样的配置，不重复执行
    acce           = m_acce;
    dece           = m_dece;
    CanDataFrameAcce( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, acce );
    CanDataFrameAcce( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, acce );
    CanDataFrameDece( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID, dece );
    CanDataFrameDece( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID, dece );
}

void DmkeMotor::DataFramesGetDriveErrInfo(CanDataFrame *frames, uint8 &i) {
    CanDataFrameGetErrInfo( frames[ i++ ], NODE_INDEX::LEFT_NODE_ID );
    CanDataFrameGetErrInfo( frames[ i++ ], NODE_INDEX::RIGHT_NODE_ID );
}

void DmkeMotor::ResetWatchDog( CanDataFrame* frames, uint8& i ){Q_UNUSED(frames)Q_UNUSED(i)}
void DmkeMotor::AlarmClean( CanDataFrame* frames, uint8& i ){Q_UNUSED(frames)Q_UNUSED(i)}

QString DmkeMotor::MotorErrInfo(NODE_INDEX index, int err_info) {
    QString msg;
    if(NODE_INDEX::LEFT_NODE_ID == index)
        msg = "错误 左轮：";
    else if(NODE_INDEX::RIGHT_NODE_ID == index)
        msg = "错误 右轮：";
        
    switch ( err_info ) {
    case 1:
        msg += "伺服驱动CRC报警 代码0";
        break;
    case 2:
        msg += "伺服驱动器内部错误 代码1";
        break;
    case 4:
        msg += "伺服驱动器短路 代码2";
        break;
    case 8:
        msg += "伺服驱动器过温 代码3";
        break;
    case 16:
        msg += "电机过温 代码4";
        break;
    case 32:
        msg += "伺服驱动器过压 代码5";
        break;
    case 64:
        msg += "伺服驱动器欠压 代码6";
        break;
    case 128:
        msg += "伺服驱动器错误 代码7";
        break;
    case 256:
        msg += "伺服驱动器相位错误 代码8";
        break;
    case 512:
        msg += "伺服驱动器跟随错误 代码9";
        break;
    case 1024:
        msg += "伺服驱动器过流 代码10";
        break;
    case 2048:
        msg += "伺服驱动器硬件故障 代码11";
        break;
    }
    return msg;
}

//private
void DmkeMotor::CanDataFrameOperationalMode( CanDataFrame& frame, uint8 value ) {
    frame.uID           = 0;
    frame.nSendType     = 0;
    frame.bRemoteFlag   = 0;
    frame.bExternFlag   = 0;
    frame.nDataLen      = 2;
    frame.arryData[ 0 ] = value;
    frame.arryData[ 1 ] = 0;
}

void DmkeMotor::CanDataFrameSpeedMode( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = speed_mode[ i ];
    }
}

void DmkeMotor::CanDataFrameControlByte0F( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = enable_mode_0F[ i ];
    }
}

void DmkeMotor::CanDataFrameControlByte00( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = disable_mode_00[ i ];
    }
}

void DmkeMotor::CanDataFrameControlByte06( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = disable_mode_06[ i ];
    }
}

void DmkeMotor::CanDataFrameAcce( CanDataFrame& frame, uint8 drive_id, long value ) {
    uint8 sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_accec[ 0 ];
    sendbuffer[ 1 ]   = set_drive_accec[ 1 ];
    sendbuffer[ 2 ]   = set_drive_accec[ 2 ];
    sendbuffer[ 3 ]   = set_drive_accec[ 3 ];
    sendbuffer[ 4 ]   = static_cast< uint8 >( value );
    sendbuffer[ 5 ]   = static_cast< uint8 >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< uint8 >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< uint8 >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void DmkeMotor::CanDataFrameDece( CanDataFrame& frame, uint8 drive_id, long value ) {
    uint8 sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_dece[ 0 ];
    sendbuffer[ 1 ]   = set_drive_dece[ 1 ];
    sendbuffer[ 2 ]   = set_drive_dece[ 2 ];
    sendbuffer[ 3 ]   = set_drive_dece[ 3 ];
    sendbuffer[ 4 ]   = static_cast< uint8 >( value );
    sendbuffer[ 5 ]   = static_cast< uint8 >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< uint8 >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< uint8 >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void DmkeMotor::CanDataFrameSetSpeedValue( CanDataFrame& frame, uint8 drive_id, long value ) {
    uint8 sendbuffer[ 8 ];
    sendbuffer[ 0 ] = set_speed_value[ 0 ];
    sendbuffer[ 1 ] = set_speed_value[ 1 ];
    sendbuffer[ 2 ] = set_speed_value[ 2 ];
    sendbuffer[ 3 ] = set_speed_value[ 3 ];
    sendbuffer[ 4 ] = static_cast< uint8 >( value );
    sendbuffer[ 5 ] = static_cast< uint8 >( value >> 8 );
    sendbuffer[ 6 ] = static_cast< uint8 >( value >> 16 );
    sendbuffer[ 7 ] = static_cast< uint8 >( value >> 24 );
    //设置速度
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void DmkeMotor::CanDataFrameGetCurVoltage( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = get_cur_voltage[ i ];
    }
}

void DmkeMotor::CanDataFrameGetCurSpeedValue( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = get_cur_speed[ i ];
    }
}

void DmkeMotor::CanDataFrameGetCurPoseValue( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = get_cur_pos[ i ];
    }
}

void DmkeMotor::CanDataFrameHomeMode( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = home_mode[ i ];
    }
}

void DmkeMotor::CanDataFrameControlByte1F( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = enable_mode_1F[ i ];
    }
}

void DmkeMotor::CanDataFrameControlByte2F( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = enable_mode_2F[ i ];
    }
}

void DmkeMotor::CanDataFrameControlByte3F( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = enable_mode_3F[ i ];
    }
}

void DmkeMotor::CanDataFramePositionMode( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = position_mode[ i ];
    }
}


void DmkeMotor::CanDataFrameSetPosModeSpeedValue( CanDataFrame& frame, uint8 drive_id, long value ) {
    uint8 sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_position_mode_speed_value[ 0 ];
    sendbuffer[ 1 ]   = set_drive_position_mode_speed_value[ 1 ];
    sendbuffer[ 2 ]   = set_drive_position_mode_speed_value[ 2 ];
    sendbuffer[ 3 ]   = set_drive_position_mode_speed_value[ 3 ];
    sendbuffer[ 4 ]   = static_cast< uint8 >( value );
    sendbuffer[ 5 ]   = static_cast< uint8 >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< uint8 >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< uint8 >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void DmkeMotor::CanDataFrameSetPoModePosValue( CanDataFrame& frame, uint8 drive_id, long value ) {
    uint8 sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_position_mode_pos_value[ 0 ];
    sendbuffer[ 1 ]   = set_drive_position_mode_pos_value[ 1 ];
    sendbuffer[ 2 ]   = set_drive_position_mode_pos_value[ 2 ];
    sendbuffer[ 3 ]   = set_drive_position_mode_pos_value[ 3 ];
    sendbuffer[ 4 ]   = static_cast< uint8 >( value );
    sendbuffer[ 5 ]   = static_cast< uint8 >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< uint8 >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< uint8 >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void DmkeMotor::CanDataFrameGetErrInfo( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = get_err_info[ i ];
    }
}

void DmkeMotor::CanDataFrameNodeProtectTimer( CanDataFrame& frame, uint8 drive_id, long value ) {
    uint8 sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_node_proetc_time[ 0 ];
    sendbuffer[ 1 ]   = set_node_proetc_time[ 1 ];
    sendbuffer[ 2 ]   = set_node_proetc_time[ 2 ];
    sendbuffer[ 3 ]   = set_node_proetc_time[ 3 ];
    sendbuffer[ 4 ]   = static_cast< uint8 >( value );
    sendbuffer[ 5 ]   = static_cast< uint8 >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< uint8 >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< uint8 >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void DmkeMotor::CanDataFrameNodeProtectFactor( CanDataFrame& frame, uint8 drive_id, long value ) {
    uint8 sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_node_proetc_factor[ 0 ];
    sendbuffer[ 1 ]   = set_node_proetc_factor[ 1 ];
    sendbuffer[ 2 ]   = set_node_proetc_factor[ 2 ];
    sendbuffer[ 3 ]   = set_node_proetc_factor[ 3 ];
    sendbuffer[ 4 ]   = static_cast< uint8 >( value );
    sendbuffer[ 5 ]   = static_cast< uint8 >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< uint8 >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< uint8 >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void DmkeMotor::CanDataFrameRemote( CanDataFrame& frame, uint8 drive_id ) {
    frame.uID         = 0x700 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 1;
    frame.bExternFlag = 0;
    frame.nDataLen    = 0;
}
